% Copyright 2020 Makani Technologies LLC
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%      http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.

function [systems, models, controllers, params] = generate_hover_controllers( ...
    with_plots)
% generate_hover_controllers -- Calculates PID gains for hover controllers.
%
% Uses MATLAB's pidtune function to calculate PID controllers for
% the individual loops in the hover controller, e.g. pitch, yaw,
% altitude, radial, and tangential position.  We attempt to meet a
% specified phase margin and gain crossover frequency, which may be
% a function of the motor's bandwidth.  Finally, this generates the
% hover_controllers.py file, which is used in the Python
% configuration scripts.

  % NOTE: Because of changes in MATLAB's pidtune algorithm, this script
  % must be run using release r2014a for consistent results.
  assert(strcmp(version('-release'), '2014a'), ...
      'This script must be run with MATLAB 2014a.');

  if nargin < 1
    with_plots = false;
  end

  wing_models = {'m600', 'oktoberkite'};
  for i = 1:length(wing_models)
    wing_model = wing_models{i};

    output_dir =  [getenv('MAKANI_HOME'), '/config/', wing_model, '/control/'];

    % Serials are in struct WingSerialToString in control/system_types.c.
    switch wing_model
      case 'm600'
        wing_serials = {'kWingSerial01', 'kWingSerial04Hover', ...
                        'kWingSerial04Crosswind', 'kWingSerial05Hover', ...
                        'kWingSerial05Crosswind', 'kWingSerial06Hover', ...
                        'kWingSerial06Crosswind', 'kWingSerial07Hover', ...
                        'kWingSerial07Crosswind'};

      case 'oktoberkite'
        wing_serials = {'kWingSerialOktoberKite01'};

        otherwise
        error(['Wing model ', wing_model, ' not recognized.']);
    end

    for k = 1:length(wing_serials)
      fprintf('\nGenerating controllers for model %s, serial number %s...\n', ...
          wing_model, wing_serials{k});
      params{k} = get_parameters(wing_model, wing_serials{k});
      models{k} = calc_models(wing_serials{k}, params{k});
      [controllers{k}.low_altitude, controllers{k}.validation_low_altitude] = ...
          calc_low_altitude_controller(models{k}, params{k});
      [controllers{k}.high_altitude, ...
       controllers{k}.validation_high_altitude] = ...
          calc_high_altitude_controller(models{k}, params{k});
      [controllers{k}.transform_tether_elevation, ...
       controllers{k}.validation_transform_tether_elevation, ...
       controllers{k}.reel_tether_elevation, ...
       controllers{k}.validation_reel_tether_elevation] = ...
          calc_tether_elevation_controllers( ...
              models{k}, controllers{k}.validation_high_altitude, params{k});
      [controllers{k}.roll, controllers{k}.validation_roll] = ...
          calc_roll_controller(models{k}, params{k});
      [controllers{k}.low_thrust_pitch, ...
       controllers{k}.validation_low_thrust_pitch] = ...
          calc_low_thrust_pitch_controller(models{k}, params{k});
      [controllers{k}.pitch, controllers{k}.validation_pitch] = ...
          calc_pitch_controller(models{k}, params{k});
      [controllers{k}.pitch_10ms, controllers{k}.validation_pitch_10ms] = ...
          calc_pitch_10ms_controller(models{k}, params{k});
      [controllers{k}.yaw, controllers{k}.validation_yaw] = ...
          calc_yaw_controller(models{k}, params{k});
      [controllers{k}.tangential_short_tether, ...
       controllers{k}.tangential_low_altitude_long_tether, ...
       controllers{k}.tangential_high_altitude_long_tether, ...
       controllers{k}.validation_tangential_short_tether, ...
       controllers{k}.validation_tangential_low_altitude_long_tether, ...
       controllers{k}.validation_tangential_high_altitude_long_tether] = ...
          calc_tangential_controller(models{k}, ...
                                     controllers{k}.validation_yaw, params{k});
      [controllers{k}.radial, controllers{k}.validation_radial] = ...
          calc_radial_controller(models{k}, ...
                                 controllers{k}.validation_pitch, params{k});
      [controllers{k}.tension_hard, controllers{k}.tension_soft] = ...
          calc_tension_controllers(models{k}, ...
                                   controllers{k}.validation_pitch, params{k});
      controllers{k}.int_pitch = ...
          calc_int_pitch_controller(models{k}.motors, models{k}.servos, ...
                                    models{k}.pitch_10ms, ...
                                    controllers{k}.validation_pitch, params{k});
      controllers{k}.int_yaw = ...
          calc_int_yaw_controller(models{k}.motors, models{k}.servos, ...
                                  models{k}.yaw, ...
                                  controllers{k}.validation_yaw, params{k});

      validate_tension_controllers(models{k}, controllers{k}.validation_pitch, ...
                                   controllers{k}.radial, ...
                                   controllers{k}.tension_hard, ...
                                   controllers{k}.tension_soft, ...
                                   10000, 1000, params{k});

      validate_tangential_controllers( ...
          models{k}, controllers{k}.validation_yaw, ...
          controllers{k}.tangential_short_tether, ...
          controllers{k}.tangential_high_altitude_long_tether, ...
          params{k});

      systems{k}.open_loop = get_open_loop_system(models{k});
    end

    if with_plots
      make_plots(wing_serials, models, controllers, params);
    end

    write_controllers([output_dir, 'hover_controllers.py'], wing_serials, ...
                      controllers);

  end
end

function params = get_parameters(wing_model, wing_serial)
  sample_time = 0.01;
  motor_bandwidth_hz = 6;

  % TODO: The servo bandwidth appears to be higher than
  % this (4 Hz as of 2016-02-24), but there's still a lot of
  % uncertainty in its model, so I'm leaving this at the conservative
  % (legacy) value for now.
  servo_bandwidth_hz = 1;

  % These coefficients can be found in:
  %     control_params.estimator.attitude.vibration_filter_[ba].
  pqr_filter = d2c(tf( ...
      [0.04915042569935978, 0.09830085139871957, 0.04915042569935978], ...
      [1.0, -1.2823928042052577, 0.4789945070026968], sample_time), ...
      'matched');

  % These coefficients can be found in:
  %     control_params.estimator.positon.filter.Vb_filter_[ba].
  wing_vel_filter = d2c(tf( ...
      [0.00934868870249279393380703595540, ...
       0.01869737740498558786761407191079, ...
       0.00934868870249279393380703595540], ...
      [1.0, ...
       -1.70846582577662697843834394006990, ...
       0.74586058058659832070702577766497], ...
      sample_time), 'matched');

  % The hover gains only require an approximate bridle knot position.
  % The actual position is found in the wing.py config file.
  if strcmp(wing_model, 'm600')
    % As of 2019-09-09 with b/140736888 the actual bridle knot position
    % for zero tether pitch is:
    % bridle_pos_b = [-0.149, -0.009, 4.916];
    bridle_pos_b = [-0.15, 0.0, 5.0];
    tether_length = 425.8;
    tether_density = 0.917;
    horizontal_tail_area = 3.49;
    horizontal_tail_pos = [-6.75, 0, -3.6];

  elseif strcmp(wing_model, 'oktoberkite')
    %TODO(b/145546641): Update these values accordingly.
    bridle_pos_b = [-0.15, 0.0, 1.0];
    tether_length = 300.0;
    tether_density = 0.917;
    horizontal_tail_area = 8.881;
    horizontal_tail_pos = [-8.5, 0.0, 0.0];

  else
    error(['Unrecognized wing model: ', wing_model]);
  end

  params = struct( ...
      'sample_time', sample_time, ...
      'tether_mass', tether_length * tether_density, ...
      'motor_bandwidth_hz', motor_bandwidth_hz, ...
      'servo_bandwidth_hz', servo_bandwidth_hz, ...
      'peg_lever_arm', [-3.0, 0.0, 0.0], ...
      'tether_length', tether_length, ...
      'min_tether_length', 5, ...
      'ascend_tension', 3000, ...
      'horizontal_tension', 3500, ...
      'horizontal_tail_area', 3.49, ...
      'horizontal_tail_pos', [-6.75, 0, -3.6], ...
      'horizontal_tail_lift_slope', 4.8, ...
      'pqr_filter', pqr_filter, ...
      'wing_vel_filter', wing_vel_filter, ...
      'g', 9.81, ...
      'rho', 1.2, ...
      'altitude_modal_damping', 0.001, ...
      'low_altitude_bandwidth_hz', motor_bandwidth_hz / 10, ...
      'low_altitude_phase_margin_deg', 80, ...
      'high_altitude_bandwidth_hz', motor_bandwidth_hz / 20, ...
      'high_altitude_phase_margin_deg', 80, ...
      'altitude_boost_integrator_bandwidth_hz', 0.1, ...
      'transform_tether_elevation_bandwidth_hz', motor_bandwidth_hz / 1000, ...
      'reel_tether_elevation_bandwidth_hz', motor_bandwidth_hz / 1000, ...
      'reel_tether_elevation_error_fc_hz', 0.05, ...
      'reel_tether_elevation_error_zeta', 1.0, ...
      'tether_elevation_phase_margin_deg', 75, ...
      'pitch_bandwidth_hz', motor_bandwidth_hz / 10, ...
      'pitch_phase_margin_deg', 60, ...
      'low_thrust_pitch_bandwidth_hz', motor_bandwidth_hz / 15, ...
      'low_thrust_pitch_phase_margin_deg', 60, ...
      'pitch_10ms_bandwidth_hz', motor_bandwidth_hz / 10, ...
      'pitch_10ms_phase_margin_deg', 60, ...
      'yaw_bandwidth_hz', motor_bandwidth_hz / 8, ...
      'yaw_phase_margin_deg', 60, ...
      'yaw_perch_bandwidth_hz', motor_bandwidth_hz / 11, ...
      'yaw_perch_phase_margin_deg', 50, ...
      'short_tether_tangential_bandwidth_hz', motor_bandwidth_hz / 40, ...
      'short_tether_tangential_phase_margin_deg', 80, ...
      'low_altitude_long_tether_tangential_bandwidth_hz', ...
      motor_bandwidth_hz / 15, ...
      'low_altitude_long_tether_tangential_phase_margin_deg', 60, ...
      'high_altitude_long_tether_tangential_bandwidth_hz', ...
      motor_bandwidth_hz / 100, ...
      'high_altitude_long_tether_tangential_phase_margin_deg', 60, ...
      'radial_bandwidth_hz', motor_bandwidth_hz / 30, ...
      'radial_phase_margin_deg', 60, ...
      'tension_bandwidth_hz', motor_bandwidth_hz / 200, ...
      'tension_phase_margin_deg', 60, ...
      'int_pitch_bandwidth_hz', motor_bandwidth_hz / 50, ...
      'int_pitch_phase_margin_deg', 60, ...
      'int_yaw_bandwidth_hz', motor_bandwidth_hz / 200, ...
      'int_yaw_phase_margin_deg', 60);


  if strcmp(wing_model, 'm600')
    switch wing_serial
      case 'kWingSerial01'
        params.wing_mass = 1648.2 + 55.3 + 10.6 + 0.6;
        params.tether_mass = 0;
        center_of_mass = [-0.054, 0.033, 0.035];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [31401.2, 47.3, 22.3; ...
                                    47.3, 8228.9, 22.3; ...
                                    22.3, 22.3, 36864.5] * ...
                                   params.wing_mass / 1606.98;

      case 'kWingSerial04Hover'
        params.wing_mass = 1746.4 + 52.1 + 10.6;
        params.tether_mass = 0;
        center_of_mass = [-0.090, 0.030, 0.097];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30270.0, 22.08, 38.92;
                                    22.08, 9221.0, 15.23;
                                    38.92, 15.23, 36630.0] * ...
                                   params.wing_mass / 1731.0;

      case 'kWingSerial04Crosswind'
        params.wing_mass = 1667.5 + 52.1 + 10.6 + 0.6;
        center_of_mass = [-0.085, 0.037, 0.108];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30260.0, 21.35, 33.90;
                                    21.35, 9210.0, 16.51;
                                    33.90, 16.51, 36620.0] * ...
                                   params.wing_mass / 1600.0;

      case 'kWingSerial05Hover'
        params.wing_mass = 1711.8 + 51.5 + 10.6;
        params.tether_mass = 0;
        center_of_mass = [-0.088, -0.011, 0.114];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30270.0, 22.08, 38.92;
                                    22.08, 9221.0, 15.23;
                                    38.92, 15.23, 36630.0] * ...
                                   params.wing_mass / 1731.0;

      case 'kWingSerial05Crosswind'
        params.wing_mass = 1629.6 + 51.5 + 10.6 + 0.6;
        center_of_mass = [-0.081, -0.006, 0.126];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30260.0, 21.35, 33.90;
                                    21.35, 9210.0, 16.51;
                                    33.90, 16.51, 36620.0] * ...
                                   params.wing_mass / 1600.0;

      case 'kWingSerial06Hover'
        params.wing_mass = 1750.2 + 51.5 + 10.6;
        params.tether_mass = 0;
        center_of_mass = [-0.093, 0.005, 0.110];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30270.0, 22.08, 38.92;
                                    22.08, 9221.0, 15.23;
                                    38.92, 15.23, 36630.0] * ...
                                   params.wing_mass / 1731.0;

      case 'kWingSerial06Crosswind'
        params.wing_mass = 1667.9 + 51.5 + 10.6 + 0.6;
        center_of_mass = [-0.086, 0.011, 0.121];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30260.0, 21.35, 33.90;
                                    21.35, 9210.0, 16.51;
                                    33.90, 16.51, 36620.0] * ...
                                   params.wing_mass / 1600.0;

      case 'kWingSerial07Hover'
        % TODO(b/145244788): The following data pertain to SN04. Update.
        params.wing_mass = 1746.4 + 52.1 + 10.6;
        params.tether_mass = 0;
        center_of_mass = [-0.090, 0.030, 0.097];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30270.0, 22.08, 38.92;
                                    22.08, 9221.0, 15.23;
                                    38.92, 15.23, 36630.0] * ...
                                   params.wing_mass / 1731.0;

      case 'kWingSerial07Crosswind'
        % TODO(b/145244788): The following data pertain to SN04. Update.
        params.wing_mass = 1667.5 + 52.1 + 10.6 + 0.6;
        center_of_mass = [-0.085, 0.037, 0.108];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [30260.0, 21.35, 33.90;
                                    21.35, 9210.0, 16.51;
                                    33.90, 16.51, 36620.0] * ...
                                   params.wing_mass / 1600.0;

      otherwise
        error('Unrecognized wing serial');
    end
  elseif strcmp(wing_model, 'oktoberkite')

    switch wing_serial
      case 'kWingSerialOktoberKite01'
        % TODO(b/145546641): Update these values accordingly.
        params.wing_mass = 1850.0;
        center_of_mass = [0.0, 0.0, 0.0];
        params.bridle_lever_arm = bridle_pos_b - center_of_mass;
        params.moment_of_inertia = [4.33e4, 0.0, 0.0;
                                    0.0, 1.76e4, 0.0;
                                    0.0, 0.0, 4.33e4] * ...
                                   params.wing_mass / 1850.0;
      otherwise
        error('Unrecognized wing serial');
    end

  else
    error('Unrecognized wing model.');
  end

end

function models = calc_models(wing_serial, params)
  % For the highest pitch model forces, we assume the thrust is at
  % maximum tether length.
  thrust = (params.wing_mass + params.tether_mass) * params.g;

  % The motor model is a simple low pass filter on the motor command.
  models.motors = tf([2 * pi * params.motor_bandwidth_hz], ...
                     [1, 2 * pi * params.motor_bandwidth_hz]);

  % The servo model is a simple low pass filter on the servo command.
  models.servos = tf([2 * pi * params.servo_bandwidth_hz], ...
                     [1, 2 * pi * params.servo_bandwidth_hz]);

  % The altitude model was chosen for the highest frequency altitude
  % dynamics near the perch where the tether tension acts as a
  % downward spring.
  mass = params.wing_mass;
  altitude_stiffness = params.ascend_tension / params.min_tether_length;
  altitude_damping = 2 * params.altitude_modal_damping ...
                     * sqrt(mass * altitude_stiffness);
  models.altitude_short_tether = tf([1], [params.wing_mass, ...
                                          altitude_damping, ...
                                          altitude_stiffness]);

  % The long tether altitude model is used to validate the tether
  % elevation controller, where the tether catenary acts as a downward
  % spring. The equivalent catenary stiffness (13 N/m) was measured during
  % hover tests at full tether length (see b/113267093).
  catenary_stiffness = 13;
  models.altitude_long_tether = tf([1], [params.wing_mass, ...
                                          altitude_damping, ...
                                          catenary_stiffness]);

  % The roll and pitch models account for the moment-of-inertia of the
  % wing and the forces from the tether action on the bridle.  The yaw
  % model is simply based around the moment-of-inertia.
  models.roll = tf([1], ...
                   [params.moment_of_inertia(1, 1), 0, ...
                    params.bridle_lever_arm(3) * params.horizontal_tension]);

  models.pitch = tf([1], [params.moment_of_inertia(2, 2), 0, ...
                          params.bridle_lever_arm(1) * thrust]);
  [k_aero, b_aero] = calc_pitch_aero_spring_damping(10, params);
  models.pitch_10ms = tf([1], [params.moment_of_inertia(2, 2), b_aero, ...
                               k_aero + params.bridle_lever_arm(1) * thrust]);

  models.yaw = tf([1], [params.moment_of_inertia(3, 3), 0, 0]);

  % The yaw model when the wing is in contact with the perch models
  % the system as an inverted pendulum that is swinging about the
  % perch peg contactor.
  I_zz_perch = params.moment_of_inertia(3, 3) ...
               + params.wing_mass * params.peg_lever_arm(1)^2;
  models.yaw_perch = tf([1], [I_zz_perch, 0, -(params.wing_mass * params.g ...
                                               * -params.peg_lever_arm(1))]);

  models.tangential = tf([1], [params.wing_mass / thrust, 0, 0]);

  % The pitch-spring and radial models change significantly as a
  % function of payout.  Here we calculate the models around an
  % operating point that roughly mimics full payout.  The spring
  % constant of 500 N/m was found empirically in the simulator.  The
  % actual spring constant can vary dramatically from around 50 N/m to
  % 1000 N/m at full payout depending on altitude.  The damping
  % constant is an order-of-magnitude estimate of the damping term at
  % low wind speeds.  It was calculated using: b = rho * C_D * A_wing
  % * v_wind, with C_D = 2, A_wing = 32, and v_wind = 1 m/s.  Some
  % damping term is necessary to make the closed-loop pitch-spring
  % model stable.
  effective_mass = params.wing_mass + params.tether_mass / 3;
  spring_constant = 500;
  damping_constant = 100;
  [models.pitch_spring, models.radial] = calc_spring_models( ...
      effective_mass, spring_constant, damping_constant, params);

  % The tension model is the transfer function between pitch and
  % tension.  We do not include tether mass as this is primarily used
  % to generate the near-perch tension controller.
  models.tension = tf([params.wing_mass * params.g], [1]);

  % The tether elevation model is the transfer function between kite
  % altitude and tether elevation angle at the GSG. It is used at full
  % tether length.
  models.tether_elevation = tf([1], [params.tether_length]);

  % A second-order low pass filter is applied to the tether elevation error
  % in reel mode.
  fc = params.reel_tether_elevation_error_fc_hz;
  wc = 2 * pi * fc;
  zeta = params.reel_tether_elevation_error_zeta;
  models.reel_tether_elevation_error_filter = tf([wc^2], ...
                                                 [1, 2 * zeta * wc, wc^2]);
end

function [k_aero, b_aero] = calc_pitch_aero_spring_damping(wind_speed, params)
  k_aero = 0.5 * params.rho * wind_speed^2 * params.horizontal_tail_area ...
           * -params.horizontal_tail_pos(3) * params.horizontal_tail_lift_slope;
  b_aero = k_aero * (-params.horizontal_tail_pos(3) / wind_speed);
end

% The pitch-spring model models the interaction of the wing's pitch
% angle and the tether modeled as a spring.
function [pitch_spring_model, radial_model] = calc_spring_models( ...
    effective_mass, spring_constant, damping_constant, params)
  thrust = (params.wing_mass + params.tether_mass) * params.g;

  % The states are: [radial, dradial/dt, pitch, dpitch/dt]'
  A = [0, 1, 0, 0;
       -spring_constant / effective_mass, ...
       -damping_constant / effective_mass, ...
       thrust / effective_mass, 0;
       0, 0, 0, 1;
       spring_constant ...
       * -params.bridle_lever_arm(1) / params.moment_of_inertia(2, 2), 0, 0, 0];
  B = [0, 0, 0, 1 / params.moment_of_inertia(2, 2)]';
  C = [0, 0, 1, 0];
  D = 0;

  % The pitch-spring model is the transfer function between pitch
  % moment and pitch.
  pitch_spring_model = tf(ss(A, B, C, D));

  % The radial model is the transfer function between pitch and radial
  % position.
  radial_model = tf([thrust], ...
                    [effective_mass, damping_constant, spring_constant]);
end

% For any of the hover compensators, the two factors keeping the hover
% controller gains from being chosen arbitrarily high are:
% 1) The maximum static thrust of each propeller.
% 2) The lag between the thrust command and the actual thrust.

% The altitude compensator is developed for the highest frequency
% altitude dynamics, which occur during launching and perching because
% of the increasing downward force from tether tension as the wing
% ascends. This downward force is modeled simply as t * z / l, where t
% is the tether tension, z is the altitude, and l is the distance from
% the wing to the tether attachment point.
function [controller, validation_controller] = ...
    calc_low_altitude_controller(models, params)
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.altitude_short_tether, ...
      params.low_altitude_bandwidth_hz, ...
      params.low_altitude_phase_margin_deg, 'pid', params.wing_vel_filter);

  % The altitude controller gets an extra boost integrator in HoverAscend.
  % Verify stability and robustness. The phase margin requirement is
  % relaxed per b/116490132.
  boost_controller_phase_margin_deg = 45;
  boost_controller = ...
      tf([1, params.altitude_boost_integrator_bandwidth_hz * 2 * pi], ...
         [1, 0]);
  validate_controller(models.motors * models.altitude_short_tether, ...
                      boost_controller * controller, ...
                      params.low_altitude_bandwidth_hz, ...
                      6, ...
                      boost_controller_phase_margin_deg);
  validate_controller(models.motors * models.altitude_short_tether, ...
                      boost_controller * validation_controller, ...
                      [], ...
                      6, ...
                      boost_controller_phase_margin_deg);
end

function [controller, validation_controller] = ...
    calc_high_altitude_controller(models, params)
  % TODO: Replace altitude_short_tether_model by
  % altitude_long_tether_model and regenerate control gains.
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.altitude_short_tether, ...
      params.high_altitude_bandwidth_hz, ...
      params.high_altitude_phase_margin_deg, 'pid', params.wing_vel_filter);
end

% Verify the stability and robustness of the tether elevation controllers
% in GsTransform and Reel modes.
function [transform_controller, validation_transform_controller, ...
          reel_controller, validation_reel_controller] = ...
        calc_tether_elevation_controllers(models, altitude_pid, params)

  % The gains of the tether elevation controllers were hand-tuned.
  transform_kp = 0.0;
  transform_ki = -16.0;
  transform_kd = 0.0;
  transform_controller = pid(transform_kp, transform_ki, transform_kd);

  reel_kp = 0.0;
  reel_ki = 0.0375;
  reel_kd = 0.0;
  reel_controller = pid(reel_kp, reel_ki, reel_kd);

  altitude_long_tether_closed = feedback( ...
      altitude_pid * models.motors * models.altitude_long_tether, 1);

  % The tether elevation PID controller used during transform modes outputs
  % the kite z-axis position offset in the ground frame. Multiply the
  % controller by -1 to get an altitude offset instead.
  plant_transform = (tf(-1) * altitude_long_tether_closed ...
                     * models.tether_elevation);
  validate_controller(plant_transform, transform_controller, ...
                      params.transform_tether_elevation_bandwidth_hz, 6, ...
                      params.tether_elevation_phase_margin_deg, 0.04);

  % The tether elevation PID controller used during reel modes outputs the
  % kite elevation angle. Assuming a straight-line tether, we multiply the
  % kite elevation by the tether length to obtain the kite altitude.
  % In addition, a second-order low pass filter is applied to the tether
  % elevation error.
  plant_reel = (models.reel_tether_elevation_error_filter ...
                * tf(params.tether_length) * altitude_long_tether_closed ...
                * models.tether_elevation);
  validate_controller(plant_reel, reel_controller, ...
                      params.reel_tether_elevation_bandwidth_hz, 6, ...
                      params.tether_elevation_phase_margin_deg, 0.04);

  derivative_filter = 1.0;
  s = tf('s');
  validation_transform_controller = transform_controller.Kp + ...
      transform_controller.Ki / s + ...
      transform_controller.Kd * s * derivative_filter;
  validate_controller( ...
      plant_transform, validation_transform_controller, [], 6, ...
      max(50, params.transform_tether_elevation_bandwidth_hz - 15), ...
      0.04, 6);
  validation_reel_controller = reel_controller.Kp + ...
      reel_controller.Ki / s + ...
      reel_controller.Kd * s * derivative_filter;
  validate_controller( ...
      plant_reel, validation_reel_controller, [], 6, ...
      max(50, params.reel_tether_elevation_bandwidth_hz - 15), 0.04, 6);
end

function [controller, validation_controller] = ...
    calc_low_thrust_pitch_controller(models, params)
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.pitch, ...
      params.low_thrust_pitch_bandwidth_hz, ...
      params.low_thrust_pitch_phase_margin_deg, 'pid', params.pqr_filter);
end

function [controller, validation_controller] = ...
    calc_pitch_controller(models, params)
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.pitch, ...
      params.pitch_bandwidth_hz, ...
      params.pitch_phase_margin_deg, 'pid', params.pqr_filter);
end

function [controller, validation_controller] = ...
    calc_pitch_10ms_controller(models, params)
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.pitch_10ms, ...
      params.pitch_10ms_bandwidth_hz, ...
      params.pitch_10ms_phase_margin_deg, 'pid', params.pqr_filter);
end

function [controller, validation_controller] = ...
    calc_roll_controller(~, ~)
  controller = pid(0.0, 0.0, 0.0);
  validation_controller = controller;
end

function [controller, validation_controller] = ...
    calc_yaw_controller(models, params)
  [controller, validation_controller] = safe_pidtune( ...
      models.motors * models.yaw, ...
      params.yaw_bandwidth_hz, ...
      params.yaw_phase_margin_deg, 'pid', params.pqr_filter);

  % Also validate the yaw controller against the yaw_perch model.
  validate_controller(models.motors * models.yaw_perch, ...
                      controller, ...
                      params.yaw_perch_bandwidth_hz, 6.0, ...
                      params.yaw_perch_phase_margin_deg, 0.04);

  validate_controller(models.motors * models.yaw_perch, ...
                      validation_controller, ...
                      [], 6.0, ...
                      max(50, params.yaw_perch_phase_margin_deg - 15), ...
                      0.04, 6.0);
end

function [short, low_long, high_long, validation_short, validation_low_long, ...
          validation_high_long] = calc_tangential_controller(models, ...
                                                             yaw_pid, params)
  yaw_closed = feedback(yaw_pid * models.motors * models.yaw, 1);
  [short, validation_short] = safe_pidtune( ...
      models.tangential * yaw_closed, ...
      params.short_tether_tangential_bandwidth_hz, ...
      params.short_tether_tangential_phase_margin_deg, ...
      'pid', params.wing_vel_filter);

  % Very slightly loosen the validation parameters for the tangential
  % outer loop.  This was necessary after the introduction of the 3.3 Hz
  % filter on the velocity estimates.
  validation_params.min_gain_margin_db = 6;
  validation_params.min_phase_margin_deg = 49;
  validation_params.min_delay_margin_s = 0.04;
  validation_params.max_sensitivity_db = 6.15;
  [low_long, validation_low_long] = safe_pidtune( ...
      models.tangential * yaw_closed, ...
      params.low_altitude_long_tether_tangential_bandwidth_hz, ...
      params.low_altitude_long_tether_tangential_phase_margin_deg, ...
      'pid', params.wing_vel_filter, validation_params);

  [high_long, validation_high_long] = safe_pidtune( ...
      models.tangential * yaw_closed, ...
      params.high_altitude_long_tether_tangential_bandwidth_hz, ...
      params.high_altitude_long_tether_tangential_phase_margin_deg, ...
      'pid', params.wing_vel_filter);
end

% At short tether lengths, we apply higher bandwidth tangential control to
% reduce tracking error.  At longer tether lengths, we reduce the bandwidth
% to avoid exciting swinging modes of the tether.  Here we validate that we
% may crossfade between these two controllers.
function validate_tangential_controllers(models, yaw_pid, ...
                                         tangential_short_pid, ...
                                         tangential_long_pid, params)
  yaw_closed = feedback(yaw_pid * models.motors * models.yaw, 1);

  for c = linspace(0, 1, 10)
    % Simply crossfading the pid objects doesn't work here, since MATLAB
    % does not recognize that the sum of two pid controllers is again a pid
    % controller.
    controller = pid((1 - c) * tangential_short_pid.Kp ...
                     + c * tangential_long_pid.Kp, ...
                     (1 - c) * tangential_short_pid.Ki ...
                     + c * tangential_long_pid.Ki, ...
                     (1 - c) * tangential_short_pid.Kd ...
                     + c * tangential_long_pid.Kd);

    % Check that the resulting controller is stable, has a bandwidth
    % intermediate between the two interpolated controllers, and meets
    % the less restrictive of the phase margin requirements.
    validate_controller( ...
        models.tangential * yaw_closed, controller, ...
        (1 - c) * params.short_tether_tangential_bandwidth_hz ...
        + c * params.high_altitude_long_tether_tangential_bandwidth_hz, ...
        6, ...
        min(params.short_tether_tangential_phase_margin_deg, ...
            params.high_altitude_long_tether_tangential_phase_margin_deg));
  end
end

% Returns a list of damping ratios for the poles of sys with
% frequencies less than max_omega.
function damping_ratios = calc_damping_ratios(sys, max_omega)
  [omegas, damping_ratios] = damp(sys);
  damping_ratios = damping_ratios(omegas < max_omega);
end

% Radial position is not directly controlled; rather, there is a
% payout command and a tension setpoint.  Together these automatically
% set a radial position.  However, at low apparent wind speeds and
% long tether lengths, the radial position is typically very
% underdamped, which can cause large overshoots and long settling
% times if the wing is started from a position far from equilibrium
% (e.g. after a switch from a piloted mode).  To address this, a
% damping term is added to the radial position loop based on the
% radial velocity.
%
% The radial damping coefficient, k_d, is chosen such that the poles
% from the closed pitch loop and the poles of the tether spring mode
% have the same damping ratio.  Specifically, we maximize the minimum
% damping ratio.  At short tether lengths, where the effective spring
% constant of the catenary is high or even dominated by the elastic
% spring constant, this will no longer work because there will not be
% sufficient phase margin.  Thus, the damping term is removed at short
% tether lengths.
function [controller, validation_controller] = ...
    calc_radial_controller(models, pitch_pid, params)
  pitch_spring_closed = feedback(pitch_pid * models.motors ...
                                 * models.pitch_spring, 1);

  % It is necessary to use minreal here because there is pole
  % cancellation between the spring part of the closed pitch-spring
  % model and the radial model, which is essentially the same spring
  % model.
  radial_open = minreal(pitch_spring_closed * models.radial);
  radial_closed = @(kd) feedback(radial_open, kd * tf('s'));

  % Maximize the minimum damping ratio.  We limit the frequencies of
  % the poles at which we calculate the damping ratios above 10 rad/s
  % in order to ignore the higher frequency poles in the 'validation
  % controller'.
  max_damping_kd = fminsearch( ...
      @(kd) -min(calc_damping_ratios(radial_closed(kd), 10)), 0.1);

  % Validate controller.  Specifically, ensure that there is still
  % sufficient phase margin with the damping.
  validate_controller(radial_open, max_damping_kd * tf('s'));
  validation_controller = max_damping_kd * tf('s') * params.wing_vel_filter;
  validate_controller(radial_open, validation_controller);

  % Only use damping term of PID (see comment above).
  controller.Kp = 0;
  controller.Ki = 0;
  controller.Kd = -max_damping_kd;
end

% The tension controller is calculated for the near perch stiff spring
% situation.
%
% It is difficult to use the stiff-spring tension controller and meet
% gain and phase margins under soft-spring conditions.  To account for
% this, we schedule the tension gain as a function of spring-constant.
function [hard, soft] = calc_tension_controllers(models, pitch_pid, params)
  pitch_closed = feedback(pitch_pid * models.motors * models.pitch, 1);
  hard = safe_pidtune(pitch_closed * models.tension, ...
                      params.tension_bandwidth_hz, ...
                      params.tension_phase_margin_deg, 'i');
  soft = hard;
  soft.Ki = hard.Ki / 10;
end

% The tension plant changes significantly during payout. We check that
% the initial, stiff-spring, tension controller works well over the
% entire range.
function validate_tension_controllers(models, pitch_pid, radial_pid, ...
                                      tension_hard_pid, tension_soft_pid, ...
                                      tension_hard, tension_soft, params)
  effective_mass = params.wing_mass + params.tether_mass / 3;
  damping_constant = 100;
  spring_constants = linspace(0, tension_hard, 50);
  for k = 1:length(spring_constants)
    [pitch_spring_model, radial_model] = calc_spring_models( ...
        effective_mass, spring_constants(k), damping_constant, params);
    pitch_spring_closed = feedback(pitch_pid * models.motors ...
                                   * pitch_spring_model, 1);
    % The tension plant is the transfer function from pitch command to
    % tension.
    tension_plant = feedback(pitch_spring_closed * radial_model, ...
                             tf([-radial_pid.Kd, 0], [1])) ...
                    * spring_constants(k);

    % Schedule the tension gain as a function of spring-constant.
    gain_scheduled_tension_pid = tension_hard_pid;
    gain_scheduled_tension_pid.Ki = interp1( ...
        [tension_hard, tension_soft], ...
        [tension_hard_pid.Ki, tension_soft_pid.Ki], ...
        spring_constants(k), 'linear', tension_soft_pid.Ki);

    if ~validate_controller(tension_plant, gain_scheduled_tension_pid, ...
                            [], 6, 30, 6, 6)
       warning(sprintf(['The tension controller did not meet ', ...
                        'specifications at k = %0.1f N/m.'], ...
                       spring_constants(k)));
    end
  end
end

% We use the elevator to try to reduce the integrated pitch error term
% in the pitch loop.
function controller = calc_int_pitch_controller(motor_model, servo_model, ...
                                                pitch_10ms_model, ...
                                                pitch_controller, params)
  % Calculate the transfer function from the elevator torque command
  % to pitch.  Note that the motor pitch controller and motor model
  % are not in the open loop path.
  pitch_with_elevator_model = servo_model ...
                              * feedback(pitch_10ms_model, ...
                                         pitch_controller * motor_model);

  % Divide by s since we are controlling the integrated pitch error.
  % Using minreal here performs pole-zero cancellation and suppresses an
  % (erroneous) warning that the system is unstable.
  controller = safe_pidtune(minreal(pitch_with_elevator_model / tf('s')), ...
                            params.int_pitch_bandwidth_hz, ...
                            params.int_pitch_phase_margin_deg, 'pi');
end

% We use the rudder to try to reduce the integrated yaw error term in
% the yaw loop.
function controller = calc_int_yaw_controller(motor_model, servo_model, ...
                                              yaw_model, yaw_controller, params)
  % Calculate the transfer function from the rudder torque command to
  % yaw. Note that the motor yaw controller and motor model are not in
  % the open loop path.
  yaw_with_rudder_model = servo_model ...
                          * feedback(yaw_model, yaw_controller * motor_model);

  % Divide by s since we are controlling the integrated yaw error.
  % Using minreal here performs pole-zero cancellation and suppresses
  % an (erroneous) warning that the system is unstable.
  controller = safe_pidtune(minreal(yaw_with_rudder_model / tf('s')), ...
                            params.int_yaw_bandwidth_hz, ...
                            params.int_yaw_phase_margin_deg, 'pi');
end

% Open-loop hover system in state-space
% - 16 states (4 actuator states + 12 rigid body dynamic states)
% - 4 inputs
% - 12 outputs
function open_loop_system = get_open_loop_system(model)
  actuators = append(ss(model.motors), ...
                     ss(model.motors), ...
                     ss(model.motors), ...
                     ss(model.motors));

  plant = append(ss(model.altitude_short_tether), ...
                 ss(model.roll), ...
                 series(ss(model.pitch), ss(model.radial)), ...
                 series(ss(model.yaw), ss(model.tangential)));

  % Perform a similarity transformation to ensure that the states of the plant
  % have a physical significance. The following logic relies on MATLAB
  % ordering of the states (state derivative then state).
  num_states = length(plant.A);
  num_inputs = size(plant.B, 2);
  num_outputs = size(plant.C, 1);
  T = eye(num_states);
  for k = 2:2:num_states
      assert(plant.A(k, k-1) ~= 0.0, ...
          ['The similarity transform is incompatible with the order of the ' ...
          'states.']);
      T(k, k) = 1 / plant.A(k, k-1);
  end
  plant = ss2ss(plant, T);

  % Scale the inputs to make the output matrix equal to the identity. The
  % following logic is only valid if all the sub-models are SISO.
  assert(issiso(model.altitude_short_tether) && issiso(model.roll) ...
      && issiso(model.pitch) && issiso(model.radial) ...
      && issiso(model.yaw) && issiso(model.tangential), ...
      'Inputs/Outputs cannot be scaled linearly for MIMO models.');
  for k = 1:num_outputs
      idx_output = find(plant.C(k, :));
      plant.B(:, k) = plant.B(:, k) .* plant.C(k, idx_output);
      plant.C(k, :) = plant.C(k, :) ./ plant.C(k, idx_output);
  end

  % Output all the states of the plant.
  C = eye(num_states);
  D = zeros(num_states, num_inputs);
  plant = ss(plant.A, plant.B, C, D);

  open_loop_system = series(actuators, plant);
  open_loop_system.InputName = { ...
      'thrust', 'motor_roll', 'motor_pitch', 'motor_yaw'};
  open_loop_system.OutputName = { ...
      'daltitude/dt', 'altitude', ...
      'droll/dt', 'roll', ...
      'dradial/dt', 'radial', ...
      'dpitch/dt', 'pitch', ...
      'dtangential/dt', 'tangential', ...
      'dyaw/dt', 'yaw'};

  % Verify that the Input/Output relationships in the combined system are the
  % same as the original I/O relationships.
  open_loop_tfs = tf(open_loop_system);
  assert(tfequal(open_loop_tfs('altitude', 'thrust'), ...
                 model.altitude_short_tether * model.motors, 1e-3));
  assert(tfequal(open_loop_tfs('roll', 'motor_roll'), ...
                 model.roll * model.motors, 1e-3));
  assert(tfequal(open_loop_tfs('radial', 'motor_pitch'), ...
                 model.radial * model.pitch * model.motors, 1e-3));
  assert(tfequal(open_loop_tfs('tangential', 'motor_yaw'), ...
                 model.tangential * model.yaw * model.motors, 1e-3));
end

function write_controllers(filename, wing_serials, controllers)
  fid = fopen(filename, 'w');
  fprintf(fid, ...
          ['"""Automatically generated hover controllers.', ...
           '\n', ...
           '\nThis file was generated by:', ...
           '\n  analysis/control/generate_hover_controllers.m', ...
           '\n"""', ...
           '\n', ...
           '\nfrom makani.control import control_types as m', ...
           '\n', ...
           '\n', ...
           '\ndef GetHoverControllers(wing_serial):', ...
           '\n  """Returns the hover controller gains."""']);
  for k = 1:length(wing_serials)
    if k == 1
      fprintf(fid, '\n  if wing_serial == m.%s:', wing_serials{k});
    else
      fprintf(fid, '\n  elif wing_serial == m.%s:', wing_serials{k});
    end

    write_controller(fid, '    ', 'low_altitude', controllers{k}.low_altitude);
    write_controller(fid, '    ', 'high_altitude', ...
                     controllers{k}.high_altitude);
    write_controller(fid, '    ', 'transform_tether_elevation', ...
                     controllers{k}.transform_tether_elevation);
    write_controller(fid, '    ', 'reel_tether_elevation', ...
                     controllers{k}.reel_tether_elevation);
    write_controller(fid, '    ', 'roll', controllers{k}.roll);
    write_controller(fid, '    ', 'low_thrust_pitch', ...
                     controllers{k}.low_thrust_pitch);
    write_controller(fid, '    ', 'pitch', controllers{k}.pitch);
    write_controller(fid, '    ', 'yaw', controllers{k}.yaw);
    write_controller(fid, '    ', 'tangential_short_tether', ...
                     controllers{k}.tangential_short_tether);
    write_controller(fid, '    ', 'tangential_low_altitude_long_tether', ...
                     controllers{k}.tangential_low_altitude_long_tether);
    write_controller(fid, '    ', 'tangential_high_altitude_long_tether', ...
                     controllers{k}.tangential_high_altitude_long_tether);
    write_controller(fid, '    ', 'radial', controllers{k}.radial);
    write_controller(fid, '    ', 'tension_hard', controllers{k}.tension_hard);
    write_controller(fid, '    ', 'tension_soft', controllers{k}.tension_soft);
    write_controller(fid, '    ', 'int_pitch', controllers{k}.int_pitch);
    write_controller(fid, '    ', 'int_yaw', controllers{k}.int_yaw);
  end
  fprintf(fid, ['\n  else:', ...
                '\n    assert False, ''wing_serial %%d was not ', ...
                'recognized'' %% wing_serial']);
  fprintf(fid, ['\n', ...
                '\n  return {', ...
                '\n      ''low_altitude'': low_altitude,', ...
                '\n      ''high_altitude'': high_altitude,', ...
                '\n      ''transform_tether_elevation'': ' ...
                'transform_tether_elevation,', ...
                '\n      ''reel_tether_elevation'': ' ...
                'reel_tether_elevation,', ...
                '\n      ''roll'': roll,', ...
                '\n      ''low_thrust_pitch'': low_thrust_pitch,', ...
                '\n      ''pitch'': pitch,', ...
                '\n      ''yaw'': yaw,', ...
                '\n      ''tangential_short_tether'': ' ...
                'tangential_short_tether,', ...
                '\n      ''tangential_low_altitude_long_tether'': (' ...
                '\n          tangential_low_altitude_long_tether),', ...
                '\n      ''tangential_high_altitude_long_tether'': (' ...
                '\n          tangential_high_altitude_long_tether),', ...
                '\n      ''radial'': radial,', ...
                '\n      ''tension_hard'': tension_hard,', ...
                '\n      ''tension_soft'': tension_soft,', ...
                '\n      ''int_pitch'': int_pitch,', ...
                '\n      ''int_yaw'': int_yaw,', ...
                '\n  }\n']);
  fclose(fid);
end

function write_controller(fid, indentation, name, controller)
  fprintf(fid, ['\n', indentation, name, ' = {', ...
                '\n', indentation, '    ''kp'': %#0.3g,', ...
                '\n', indentation, '    ''ki'': %#0.3g,', ...
                '\n', indentation, '    ''kd'': %#0.3g', ...
                '\n', indentation, '}'], ...
          controller.Kp, controller.Ki, controller.Kd);
end

function make_plots(wing_serials, models, controllers, params)
  for k = 1:length(models)

    % Open/Closed loop models
    roll_open = models{k}.motors * models{k}.roll * controllers{k}.roll;
    roll_closed = feedback(roll_open, 1);
    pitch_open = models{k}.motors * models{k}.pitch * controllers{k}.pitch;
    pitch_closed = feedback(pitch_open, 1);
    pitch_10ms_open = ...
        models{k}.motors * models{k}.pitch_10ms * controllers{k}.pitch_10ms;
    pitch_10ms_closed = feedback(pitch_10ms_open, 1);
    yaw_open = models{k}.motors * models{k}.yaw * controllers{k}.yaw;
    yaw_closed = feedback(yaw_open, 1);
    altitude_low_open = models{k}.motors * models{k}.altitude_short_tether ...
        * controllers{k}.low_altitude;
    altitude_low_closed = feedback(altitude_low_open, 1);
    altitude_high_open = models{k}.motors * models{k}.altitude_short_tether ...
        * controllers{k}.high_altitude;
    altitude_high_closed = feedback(altitude_high_open, 1);
    altitude_long_tether_open = models{k}.motors * ...
        models{k}.altitude_long_tether * controllers{k}.high_altitude;
    altitude_long_tether_closed = feedback(altitude_long_tether_open, 1);
    transform_tether_elevation_open = ...
        controllers{k}.transform_tether_elevation * tf(-1) ...
        * altitude_long_tether_closed * models{k}.tether_elevation;
    transform_tether_elevation_closed = feedback( ...
        transform_tether_elevation_open, 1);
    reel_tether_elevation_open = ...
        models{k}.reel_tether_elevation_error_filter ...
        * controllers{k}.reel_tether_elevation * tf(params{k}.tether_length) ...
        * altitude_long_tether_closed * models{k}.tether_elevation;
    reel_tether_elevation_closed = feedback(reel_tether_elevation_open, 1);
    tangential_short_open = models{k}.tangential * yaw_closed ...
        * controllers{k}.tangential_short_tether;
    tangential_short_closed = feedback(tangential_short_open, 1);
    tangential_low_long_open = models{k}.tangential * yaw_closed ...
        * controllers{k}.tangential_low_altitude_long_tether;
    tangential_low_long_closed = feedback(tangential_low_long_open, 1);
    tangential_high_long_open = models{k}.tangential * yaw_closed ...
        * controllers{k}.tangential_high_altitude_long_tether;
    tangential_high_long_closed = feedback(tangential_high_long_open, 1);

    % Step responses
    angle_str = sprintf('Angle loop step responses for %s', ...
                        wing_serials{k});
    figure('Name', angle_str);
    clf;
    stepplot(roll_closed, pitch_closed, yaw_closed);
    xlim([0, 20])
    grid on;
    title(angle_str);
    legend('roll', 'pitch', 'yaw');

    position_str = sprintf('Position loop step responses for %s', ...
                           wing_serials{k});
    figure('Name', position_str);
    clf;
    stepplot(altitude_low_closed, altitude_high_closed, ...
        altitude_long_tether_closed, tangential_short_closed, ...
        tangential_low_long_closed, tangential_high_long_closed);
    xlim([0, 20])
    grid on;
    title(position_str);
    legend('altitude (low)', 'altitude (high)', ...
        'altitude (long tether)', 'tangential (short tether)', ...
        'tangential (long tether, low altitude)', ...
        'tangential (long tether, high altitude)', 'Location', 'SouthEast');

    % Bode plots
    altitude_bode_str = sprintf( ...
        'Altitude control Bode plot for %s', wing_serials{k});
    figure('Name', altitude_bode_str)
    bode(altitude_low_open, altitude_high_open, altitude_long_tether_open);
    grid on;
    title(altitude_bode_str);
    legend('altitude (low)', 'altitude (high)', 'altitude (long tether)');

    tether_elevation_bode_str = sprintf(...
        'Tether elevation control Bode plot for %s', wing_serials{k});
    figure('Name', tether_elevation_bode_str)
    bode(transform_tether_elevation_open, reel_tether_elevation_open);
    grid on;
    title(tether_elevation_bode_str);
    legend('tether elevation (transform)', 'tether elevation (reel)');
  end
end

function result = tfequal(tf_a, tf_b, tol)
  % Check whether two linear systems represented by their transfer function
  % are the same.
  [num, den] = tfdata(tf_a / tf_b, 'v');
  result = all(abs(num - den) < tol);
end
